import { Euler, Matrix3, Matrix4, Vector3 } from "three";
import { degToRad } from "three/src/math/MathUtils.js";

function XY2Theta(x, y) {
    const r = Math.sqrt(x * 2 + y * y);
    const theta = Math.atan2(y, x);
    return { r, theta };
}

export default class SlamCamera {
    constructor(options) {
        const { distortionCoeffs, external, internal } = options;
        const fx = this._fx = internal.fx;
        const fy = this._fy = internal.fy;
        const cy = this._cy = internal.cy;
        const cx = this._cx = internal.cx;

        this._k = new Matrix3(fx, 0, cx, 0, fy, cy, 0, 0, 1);

        this._external = new Matrix4().fromArray(external);
        // this._external = this._external.invert();

        this._k1 = distortionCoeffs[0];
        this._k2 = distortionCoeffs[1];
        this._k3 = distortionCoeffs[2];
        this._p1 = distortionCoeffs[3];
        this._p2 = distortionCoeffs[4];
    }
    /**
     * 
     * @param {Vector3} pos 
     */
    Compute(pos) {
        const camepos = pos.clone().applyMatrix4(this._external);
        const { x, y, z } = camepos;
        const r2 = x * x + y * y;
        const x_distorted = x * (1 + this._k1 * r2 + this._k2 * r2 * r2) + 2 * this._p1 * x * y + this._p2 * (r2 + 2 * x * x);
        const y_distorted = y * (1 + this._k1 * r2 + this._k2 * r2 * r2) + this._p1 * r2 + 2 * y * y + 2 * this._p2 * x * y;
        const u = this._fx * (x_distorted / z) + this._cx;
        const v = this._fy * (y_distorted / z) + this._cy;
        return { u, v }
    }

    /**
     * 
     * @param {Vector3} pos 
     */
    GetUV(pos) {
        const p = pos.clone();
        p.multiplyScalar(1.0 / pos.z)
        return this._kp * p;
    }

    GetDistoredUV(x, y) {
        const { r, theta } = XY2Theta(x, y);
        const r2 = r * r;
        const r4 = r2 * r2;
        const r6 = r4 * r2;
        const xy = x * y;
        const k123 = 1 + this._k1 * r2 + this._k2 * r4 + this._k3 * r6;

        const x_distorted = x * k123 + 2 * this._p1 * xy + this._p2(r2 + 2 * x * x);
        const y_distorted = y * k123 + this._p1(r2 + 2 * y * y) + 2 * this._p2 * xy;

        const u = this._fx * x_distorted + this._cx;
        const v = this._fy * y_distorted + this._cy;
        return { u, v };
    }
}